home *** CD-ROM | disk | FTP | other *** search
/ Beginning Mac Programming / Beginning Mac Programming.bin / pc / Open Me for REALbasic 3 / REALbasic 3.2 / Goodies / 3rd Party Demos / 3rd Party Plugins / Misc / PEVocoder 1.0 (PPC) / PEVocodePlugin Source Code / fftn.cp < prev    next >
Encoding:
Text File  |  2000-11-21  |  17.0 KB  |  482 lines
  1. /* $Id: fftn.c,v 1.2 1998/09/13 00:21:18 emanuel Exp $ */
  2.  
  3. /* (C) Copyright 1993 by Steven Trainoff.  Permission is granted to make
  4. * any use of this code subject to the condition that all copies contain
  5. * this notice and an indication of what has been changed.
  6. */
  7. #include <stdio.h>
  8. #include <math.h>
  9. #include "error.h"
  10. #include "spt.h"
  11.  
  12. #ifndef REAL
  13. #define REAL double
  14. #endif
  15.  
  16. #include "fft.h"
  17.         
  18. #ifndef PI
  19. #define PI      3.14159265358979324
  20. #endif
  21.  
  22. #define emalloc error_malloc
  23.  
  24. /* This routine performs a complex fft.  It takes two arrays holding
  25.  * the real and imaginary parts of the the complex numbers.  It performs
  26.  * the fft and returns the result in the original arrays.  It destroys
  27.  * the orginal data in the process.  Note the array returned is NOT
  28.  * normalized.  Each element must be divided by n to get dimensionally
  29.  * correct results.  This routine takes optional arrays for the sines, cosine
  30.  * and bitreversal.  If any of these pointers are null, all of the arrays are 
  31.  * regenerated.
  32.  */
  33.  
  34. //void fft(x, n, c, s, rev)
  35.  
  36. //REAL x[][2];            /* Input data points */
  37. //int n;                  /* Number of points, n = 2**nu */
  38. //REAL *c, *s;            /* Arrays of cosine, sine */
  39. //int *rev;                       /* Array of bit reversals*/
  40. void fft(REAL x[][2], int n, REAL *c, REAL *s, int *rev){
  41.         char temparray=FALSE;       /* Are we using internal c and s arrays? */
  42.         int nu = ilog2(n);      /* Number of data points */
  43.         int dual_space = n;     /* Spacing between dual nodes */
  44.         int nu1 = nu;           /* = nu-1 right shift needed when finding p */
  45.         int k;                          /* Iteration of factor array */
  46.         register int i;                 /* Number of dual node pairs considered */
  47.         register int j;                 /* Index into factor array */
  48.         
  49.         if (c == NULL || s == NULL || rev == NULL) {
  50.                 temparray = TRUE;
  51.                 fft_create_arrays(&c, &s, &rev, n);
  52.         }
  53.         
  54.         /* For each iteration of factor matrix */
  55.         for (k = 0; k < nu; k++) {
  56.                 /* Initialize */
  57.                 dual_space /= 2;        /* Fewer elements in each set of duals */
  58.                 nu1--;                  /* nu1 = nu - 1 */
  59.  
  60.                 /* For each set of duals */
  61.                 for(j = 0; j < n; j += dual_space) {
  62.                         /* For each dual node pair */
  63.                         for (i = 0; i < dual_space; i++, j++) {
  64.                                 REAL treal, timag;              /* Temp of w**p */
  65.                                 register int p = rev[j >> nu1];
  66.                                 
  67.                                 treal = x[j+dual_space][0]*c[p] + x[j+dual_space][1]*s[p];
  68.                                 timag = x[j+dual_space][1]*c[p] - x[j+dual_space][0]*s[p];
  69.  
  70.                                 x[j+dual_space][0] = x[j][0] - treal;
  71.                                 x[j+dual_space][1] = x[j][1] - timag;
  72.  
  73.                                 x[j][0] += treal;
  74.                                 x[j][1] += timag;
  75.                         }
  76.                 }
  77.         }
  78.  
  79.         /* We are done with the transform, now unscamble results */
  80.         for (j = 0; j < n; j++) {
  81.                 if ((i = rev[j]) > j) {
  82.                         REAL treal, timag;
  83.  
  84.                         /* Swap */
  85.                         treal = x[j][0];
  86.                         timag = x[j][1];
  87.  
  88.                         x[j][0] = x[i][0];
  89.                         x[j][1] = x[i][1];
  90.  
  91.                         x[i][0] = treal;
  92.                         x[i][1] = timag;
  93.                 }
  94.         }
  95.                     
  96.          /* Give back the temp storage */
  97.         if (temparray) {
  98.             free(c);
  99.             free(s);
  100.             free(rev);
  101.         }
  102. }
  103.  
  104. /* invfft performs an inverse fft */
  105. void invfft(REAL (*x)[2], int n, REAL*c, REAL*s, int *rev)
  106. {
  107.         char temparray = FALSE;
  108.         int i;
  109.         
  110.         if (c == NULL || s == NULL || rev == NULL) {
  111.                 temparray = TRUE;
  112.                 fft_create_arrays(&c, &s, &rev, n);
  113.         }
  114.         
  115.         /* Negate the sin array to do the inverse transform */
  116.         for (i = 0; i < n; i++)
  117.                 s[i] = -s[i];
  118.         
  119.         fft(x, n, c, s, rev);
  120.         
  121.         if (temparray) {
  122.                 free(c);
  123.                 free(s);
  124.                 free(rev);
  125.         } else {                        /* Put the sin array back */
  126.                 for (i = 0; i < n; i++)
  127.                         s[i] = - s[i];
  128.         }
  129. }
  130.  
  131.  
  132.  
  133. /* fft_create_arrays generates the sine and cosine arrays needed in the
  134.  * forward complex fft.  It allocates storaged and return pointers
  135.  * to the initialized arrays 
  136.  */
  137.  
  138. //void fft_create_arrays(c, s, rev, n)
  139. void fft_create_arrays(REAL **c, REAL **s, int **rev, int n)
  140. //REAL **c, **s;          /* Sin and Cos arrays (to be returned) */
  141. //int n;                          /* Number of points */
  142. //int **rev;                      /* Array of reversed bits */
  143. {
  144.     register int i;
  145.     int nu = ilog2(n);
  146.  
  147.     /* Compute temp array of sins and cosines */
  148.     *c = (REAL *)emalloc(n * sizeof(REAL));
  149.     *s = (REAL *)emalloc(n * sizeof(REAL));
  150.     *rev = (int *)emalloc(n * sizeof(int));
  151.     
  152.     for (i = 0; i < n; i++) {
  153.         REAL arg = 2 * PI * i/n;
  154.         (*c)[i] = cos(arg);
  155.         (*s)[i] = sin(arg);
  156.         (*rev)[i] = bitrev(i, nu);
  157.     }
  158. }
  159.  
  160. #if (1==2)                      /* Not needed now */
  161. /* getx - gets one real, imag pair from the square multidimensional array */
  162. REAL *getx(x, ndim, dim, n, elem)
  163. REAL x[][2];                    /* Input data points */
  164. int ndim;                       /* Number of dimensions */
  165. int dim;                        /* Dim to extract point from */
  166. int n;                          /* Number of points in each dim */
  167. int elem[];                     /* Which element to extract (array by ndim) */
  168. {
  169.     register int i;
  170.     int pos=0;                  /* Position in array when considered 1d */
  171.  
  172.     for (i = 0; i < ndim && (pos *= n); i++)  /* Loop over dimensions */
  173.         pos += elem[i];
  174.  
  175.     return(x+pos);
  176. }
  177. #endif
  178.  
  179. /* This routine performs a multiple complex fft on a square
  180.  * multidimensional array.  Each element is an array by two holding the
  181.  * real and imaginary parts of the the complex numbers.  It performs the
  182.  * fft and returns the result in the original arrays.  It destroys the
  183.  * orginal data in the process.  Note the array returned is NOT
  184.  * normalized.  Each element must be divided by n to get dimensionally
  185.  * correct results.  The cosine and sine arrays are optional.  If null is
  186.  * passed, a temp array will be allocated and filled, otherwise the
  187.  * passed arrays are assumed to have the correct data in them.
  188.  *
  189.  * Note: if the sin array is negated, the routine performs the inverse
  190.  * transform.
  191.  */
  192.  
  193. //void fftn(x, ndim, dim)
  194. void fftn(REAL x[][2], int ndim, int dim[])
  195. //REAL x[][2];            /* Input data points */
  196. //int ndim;                       /* Number of dimensions */
  197. //int dim[];                      /* Number of points in each dim = 2**nu */
  198. {
  199.     int nel;                    /* Number of elements */
  200.     int separation = 1;         /* Distance between elements (which "column") */
  201.     int offset = 0;             /* 1st element in fft (which "row") */
  202.     register int i;             /* Which dim we are performing the fft */
  203.     register int j;             /* Which indices we are NOT performing the fft */
  204.     register int k;             /* Loop over index j */
  205.     REAL *c, *s;                /* Sine and Cosine arrays */
  206.     int *rev;                   /* Array of bitreversed numbers */
  207.  
  208.     /* Compute number of elements in array */
  209.     for (i = 0, nel = 1; i < ndim; i++)
  210.         nel *= dim[i];
  211.     
  212.     for (i = ndim-1; i >= 0; i--) { /* Loop over dim on which we are doing the fft */
  213.         int logi = ilog2(dim[i]);
  214.         int nextseparation = separation * dim[i];
  215.  
  216.         /* Create Sin and Cos arrays */
  217.         fft_create_arrays(&c, &s, &rev, dim[i]);
  218.     
  219.         /* Loop over other indices.  First do indicies bigger than i */
  220.         for (j =0; j < nel; j += nextseparation) {
  221.             for (k = 0; k < separation; k++) {
  222.                 offset = j+k;
  223.                 fft1n(x, logi, offset, separation, c, s, rev);
  224.             }
  225.         }
  226.         /* Give back old sin and cos arrays */
  227.         free(c); free(s);free(rev);
  228.         separation = nextseparation;
  229.     }
  230. }
  231.  
  232. /* This routine is identical to fftn but negates the sin array to do an inverse transform */
  233.  
  234. //void invfftn(x, ndim, dim)
  235. void invfftn(REAL x[][2], int ndim, int dim[])
  236. //REAL x[][2];            /* Input data points */
  237. //int ndim;                       /* Number of dimensions */
  238. //int dim[];                      /* Number of points in each dim = 2**nu */
  239. {
  240.     int nel;                    /* Number of elements */
  241.     int separation = 1;         /* Distance between elements (which "column") */
  242.     int offset = 0;             /* 1st element in fft (which "row") */
  243.     register int i;             /* Which dim we are performing the fft */
  244.     register int j;             /* Which indices we are NOT performing the fft */
  245.     register int k;             /* Loop over index j */
  246.     REAL *c, *s;                /* Sine and Cosine arrays */
  247.     int *rev;                   /* Array of bitreversed numbers */
  248.  
  249.     /* Compute number of elements in array */
  250.     for (i = 0, nel = 1; i < ndim; i++)
  251.         nel *= dim[i];
  252.     
  253.     for (i = ndim-1; i >= 0; i--) { /* Loop over dim on which we are doing the fft */
  254.         int logi = ilog2(dim[i]);
  255.         int nextseparation = separation * dim[i];
  256.  
  257.         /* Create Sin and Cos arrays */
  258.         fft_create_arrays(&c, &s, &rev, dim[i]);
  259.         
  260.         /* Negate the sin array */
  261.         for (j = 0; j < dim[i]; j++)
  262.                 s[j] = -s[j];
  263.     
  264.         /* Loop over other indices.  First do indicies bigger than i */
  265.         for (j =0; j < nel; j += nextseparation) {
  266.             for (k = 0; k < separation; k++) {
  267.                 offset = j+k;
  268.                 fft1n(x, logi, offset, separation, c, s, rev);
  269.             }
  270.         }
  271.         /* Give back old sin and cos arrays */
  272.         free(c); free(s);free(rev);
  273.         separation = nextseparation;
  274.     }
  275. }
  276.  
  277. /* This normalizes the elements of a multidimensional fft so that the forward transform
  278.  * followed by the inverse transform is an identity
  279.  */
  280. //void normalize_fftn(x, ndim, dim)
  281. void normalize_fftn(REAL x[][2], int ndim, int dim[])
  282. //REAL x[][2];            /* Input data points */
  283. //int ndim;                       /* Number of dimensions */
  284. //int dim[];                      /* Number of points in each dim = 2**nu */
  285. {
  286.     int nel;            /* Number of elements */
  287.     int i;
  288.     
  289.     /* Compute number of elements in array */
  290.     for (i = 0, nel = 1; i < ndim; i++)
  291.         nel *= dim[i];
  292.  
  293.     for (i = 0; i < nel; i++) {
  294.         x[i][0] /= nel;
  295.         x[i][1] /= nel;
  296.     }
  297. }
  298.  
  299.  /* This routine normalized the elements of a 1d fft by dividing by the number of elements
  300.   * so that fft, inversefft, normalizefft is an identity
  301.   */
  302. void normalize_fft(REAL (*x)[2], int n)
  303. {
  304.     register int i;
  305.     
  306.     for (i = 0; i < n; i++) {
  307.         x[i][0] /= n;
  308.         x[i][1] /= n;
  309.     }
  310. }
  311.  
  312.  
  313. /* This routine performs a complex fft on a single index of a
  314.  * multidimensional array.  This routine is intended to be used
  315.  * internally in a full multidimensional fft (on all indicies).  It will
  316.  * be called repeatedly for each of the 1D fft's needed.  This routine is
  317.  * designed primarily to optimize the memory fetches needed for the 1D
  318.  * ffts.  Each element is an array by two holding the real and imaginary
  319.  * parts of the the complex numbers.  Which index on which the fft is to
  320.  * be performed is specified in a somewhat roundabout fashion.  What is
  321.  * passed it an offset and the number of values between elements in the
  322.  * array as if it were considered to be a big 1D array with dimension
  323.  * equal to the product of the linear dimensions.  If the
  324.  * multidimensional array is considered It performs the fft and returns
  325.  * the result in the original arrays.  It destroys the orginal data in
  326.  * the process.  Note the array returned is NOT normalized.  Each element
  327.  * must be divided by n to get dimensionally correct results.  The cosine
  328.  * and sine arrays are optional.  If null is passed, a temp array will be
  329.  * allocated and filled, otherwise the passed arrays are assumed to have
  330.  * the correct data in them.
  331.  * 
  332.  * Note: if the sin array is negated, the routine performs the inverse
  333.  * transform.
  334.  */
  335.  
  336. //void fft1n(x, nu, offset, separation, c, s, rev)
  337. void fft1n(REAL x[][2], int nu, int offset, int separation, REAL c[], REAL s[], int rev[])
  338. //REAL x[][2];            /* Input data points */
  339. //int nu;                         /* Number of elements in fft n = 2**nu */
  340. //int offset;                     /* Offset of 1st element */
  341. //int separation;                 /* Separation between elements */
  342. //REAL c[], s[];          /* Cosine and Sine arrays (array by n) */
  343. //int rev[];                      /* Array of bitreversed numbers */
  344. {
  345.     char temparray=FALSE;       /* Are we using internal c and s arrays? */
  346.     int n = (1 << nu);          /* Number of data points */
  347.     int dual_space = n; /* Spacing between dual nodes */
  348.     int nu1 = nu;               /* = nu-1 right shift needed when finding p */
  349.     int k;                              /* Iteration of factor array */
  350.     register int i;                     /* Number of dual node pairs considered */
  351.     register int j;             /* Index into factor array */
  352.     
  353.     if (c == NULL || s == NULL || rev == NULL) {
  354.         temparray = TRUE;
  355.         fft_create_arrays(&c, &s, &rev, n);
  356.     }
  357.     
  358.     x += offset;                /* Move to the correct offset */
  359.  
  360.     /* For each iteration of factor matrix */
  361.     for (k = 0; k < nu; k++) {
  362.         /* Initialize */
  363.         dual_space /= 2;        /* Fewer elements in each set of duals */
  364.         nu1--;                  /* nu1 = nu - 1 */
  365.         
  366.         /* For each set of duals */
  367.         for(j = 0; j < n; j += dual_space) {
  368.             
  369.             /* For each dual node pair */
  370.             for (i = 0; i < dual_space; i++, j++) {
  371.                 REAL *pt1, *pt2;
  372.                 REAL treal, timag;      /* Temp of w**p */
  373.                 register int p = rev[j >> nu1];
  374.                 
  375.                 pt1 = x[separation * j];
  376.                 pt2 = x[separation * (j+dual_space)];
  377.  
  378.                 treal = pt2[0]*c[p] + pt2[1]*s[p];
  379.                 timag = pt2[1]*c[p] - pt2[0]*s[p];
  380.                 
  381.                 pt2[0] = pt1[0] - treal;
  382.                 pt2[1] = pt1[1] - timag;
  383.                 
  384.                 pt1[0] += treal;
  385.                 pt1[1] += timag;
  386.             }
  387.         }
  388.     }
  389.     
  390.     
  391.     /* We are done with the transform, now unscamble results */
  392.     for (j = 0; j < n; j++) {
  393.         if ((i = rev[j]) > j) {
  394.             REAL *pt1, *pt2;
  395.             REAL treal, timag;
  396.  
  397.             pt1 = x[j*separation];
  398.             pt2 = x[i*separation];
  399.             /* Swap */
  400.             treal = pt1[0];
  401.             timag = pt1[1];
  402.             
  403.             pt1[0] = pt2[0];
  404.             pt1[1] = pt2[1];
  405.             
  406.             pt2[0] = treal;
  407.             pt2[1] = timag;
  408.         }
  409.     }
  410.     
  411.      /* Give back the temp storage */
  412.     if (temparray) {
  413.         free(c);
  414.         free(s);
  415.         free(rev);
  416.     }
  417. }
  418.  
  419. /* This routine takes an array of real numbers and performs a fft.  It
  420.  * returns the magnitude of the fft in the original array.  This routine
  421.  * uses an order n/2 complex ft and disentangles the results.  This is
  422.  * much more efficient than using an order n complex fft with the
  423.  * imaginary component set to zero.  We return the mean in data[0]
  424.  * and the Nyquist frequency in data[n/w].  The rest of data is
  425.  * left untouched.  The results are normalized.
  426.  */
  427.  
  428. //void realfftmag(data, n)
  429. void realfftmag(REAL *data, int n)
  430. //REAL *data;
  431. //int n;
  432. {
  433.         REAL (*x)[2];                   /* Temp array used perform fft */
  434.         REAL *dataptr, *xptr;   /* Temp pointer into data array */
  435.         int i;
  436.  
  437.         x = (REAL (*)[2])emalloc(n * sizeof(REAL));
  438.         
  439.         /* Load data into temp array
  440.          * even terms end up in x[n][0] odd terms in x[n][1]
  441.          */
  442.         for (i = 0, dataptr = data, xptr = (REAL *)x; i < n; i++) 
  443.                 *xptr++ = *dataptr++;
  444.         
  445.         fft(x, n/2, NULL, NULL, NULL);
  446.  
  447.         /* Load results into output array */
  448.  
  449.         /* i = 0 needs to be treated separately */
  450.         data[0] = (x[0][0] + x[0][1])/n;
  451.  
  452.         for (i = 1; i < n/2; i++) {
  453.                 double xr, xi;
  454.                 double  arg, ti, tr;
  455.                 double c, s;            /* Cosine and sin */
  456.  
  457.                 arg = 2 * PI * i / n;
  458.                 c = cos(arg);   /* These are different c,s than used in fft */
  459.                 s = sin(arg);
  460.  
  461.                 ti = (x[i][1] + x[n/2-i][1]) / 2;
  462.                 tr = (x[i][0] - x[n/2-i][0]) / 2;
  463.  
  464.                 xr = (x[i][0] + x[n/2-i][0])/2 + c * ti - s * tr;
  465.                 xi = (x[i][1] - x[n/2-i][1])/2 - s * ti - c * tr;
  466.  
  467.                 xr /= n/2;
  468.                 xi /= n/2;
  469.                 
  470.                 data[i] = sqrt(sqr(xr) + sqr(xi));
  471.         }
  472.         
  473.         /* Nyquist frequency is returned in data[0] */
  474.         data[n/2] = (x[0][0] - x[0][1])/n;
  475.         
  476.         free(x);
  477. }
  478.         
  479.  
  480.  
  481.  
  482.